#include <opencv2/features2d.hpp>
#include <opencv2/opencv.hpp>
#include <opencv2/highgui.hpp>      //for imshow
#include <vector>
#include <iostream>
#include <iomanip>

#include "stats.h" // Stats structure definition
#include "utils.h" // Drawing and printing functions

using namespace std;
using namespace cv;

const double akaze_thresh = 3e-4; // AKAZE detection threshold set to locate about 1000 keypoints
const double ransac_thresh = 2.5f; // RANSAC inlier threshold
const double nn_match_ratio = 0.8f; // Nearest-neighbour matching ratio
const int bb_min_inliers = 100; // Minimal number of inliers to draw bounding box
const int stats_update_period = 10; // On-screen statistics are updated every 10 frames

#include <sys/select.h>  
#include <sys/socket.h>  
#include <arpa/inet.h>  
#include <unistd.h>

#define MAX_DATA_BUF_LEN 1024000

int sd = 0;  
sockaddr_in addr_org; // 发送端地址  

void initSocket() {
    sd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP); // 创建socket  

    addr_org.sin_family = AF_INET;  
    addr_org.sin_addr.s_addr = inet_addr("127.0.0.1"); // 发送端IP  
    addr_org.sin_port = htons(2222); // 发送端端口  
      
    bind(sd, (struct sockaddr *)&(addr_org), sizeof(struct sockaddr_in)); // 绑定IP和端口 
}

bool socketSend(const char* SendDataBuff, int ulLen) {
    sockaddr_in addr_dst; // 接收端地址  
  
    addr_dst.sin_family = AF_INET;  
    addr_dst.sin_addr.s_addr = inet_addr("127.0.0.1"); // 接收端IP  
    addr_dst.sin_port = htons(1111); // 接收端端口  
      
    //... // 生成要发送的数据到SendDataBuff, 长度为ulLen  
    sendto(sd, SendDataBuff, ulLen, 0, (struct sockaddr *)&(addr_dst), sizeof(sockaddr_in)); // 发送SendDataBuff中的数据  
    return true;
}

namespace example {
class Tracker
{
public:
    Tracker(Ptr<Feature2D> _detector, Ptr<DescriptorMatcher> _matcher) :
        detector(_detector),
        matcher(_matcher)
    {}

    void setFirstFrame(const Mat frame, Stats& stats);
    Mat process(Stats& stats, const vector<KeyPoint>& kp, const Mat& desc);
    Ptr<Feature2D> getDetector() {
        return detector;
    }
protected:
    Ptr<Feature2D> detector;
    Ptr<DescriptorMatcher> matcher;

    Mat first_frame, first_desc;
    vector<KeyPoint> first_kp;
};

void Tracker::setFirstFrame(const Mat frame, Stats& stats)
{
    first_frame = frame.clone();
    detector->detectAndCompute(first_frame, noArray(), first_kp, first_desc);
    stats.keypoints = (int)first_kp.size();
}

Mat Tracker::process(Stats& stats, const vector<KeyPoint>& kp, const Mat& desc)
{
    vector< vector<DMatch> > matches;
    vector<KeyPoint> matched1, matched2;
    matcher->knnMatch(first_desc, desc, matches, 2);
    for(unsigned i = 0; i < matches.size(); i++) {
        if(matches[i][0].distance < nn_match_ratio * matches[i][1].distance) {
            matched1.push_back(first_kp[matches[i][0].queryIdx]);
            matched2.push_back(      kp[matches[i][0].trainIdx]);
        }
    }
    stats.matches = (int)matched1.size();

    Mat inlier_mask, homography;
    vector<KeyPoint> inliers1, inliers2;
    vector<DMatch> inlier_matches;
    if(matched1.size() >= 4) {
        homography = findHomography(Points(matched1), Points(matched2),
                                    RANSAC, ransac_thresh, inlier_mask);
    }

    if(matched1.size() < 4 || homography.empty()) {
        Mat res;
        //hconcat(first_frame, frame, res);
        stats.inliers = 0;
        stats.ratio = 0;
        return res;
    }
    for(unsigned i = 0; i < matched1.size(); i++) {
        if(inlier_mask.at<uchar>(i)) {
            int new_i = static_cast<int>(inliers1.size());
            inliers1.push_back(matched1[i]);
            inliers2.push_back(matched2[i]);
            inlier_matches.push_back(DMatch(new_i, new_i, 0));
        }
    }
    stats.inliers = (int)inliers1.size();
    stats.ratio = stats.inliers * 1.0 / stats.matches;

/*
    vector<Point2f> new_bb;
    perspectiveTransform(object_bb, new_bb, homography);
    Mat frame_with_bb = frame.clone();
    if(stats.inliers >= bb_min_inliers) {
        drawBoundingBox(frame_with_bb, new_bb);
    }
    Mat res;
    drawMatches(first_frame, inliers1, frame_with_bb, inliers2,
                inlier_matches, res,
                Scalar(255, 0, 0), Scalar(255, 0, 0));
*/
    Mat res;
    //socketSend((char*)&res, 0);
    char buf[128] = "server server";
    socketSend(buf, sizeof(buf));
    printf("send server server");
    return res;
}
}

int main(int argc, char **argv)
{
    Stats stats, akaze_stats;
    Ptr<AKAZE> akaze = AKAZE::create();
    akaze->setThreshold(akaze_thresh);
    Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");
    example::Tracker akaze_tracker(akaze, matcher);
    
    Mat frame = imread("object.jpg");
    akaze_tracker.setFirstFrame(frame, stats);

    initSocket();

    Stats akaze_draw_stats;
    Mat akaze_res, res_frame;

    fd_set fdset;  
    struct timeval delayval;  
    unsigned long lRetVal;  
  
    delayval.tv_sec = 0;  
    delayval.tv_usec = 5000; /*5ms延时*/  
  
    while (sd) {  
        FD_ZERO(&fdset);
        FD_SET(sd, &fdset);
  
        lRetVal = select(sd+1, &fdset, NULL, NULL, &delayval); // 尤其要注意第一个参数
        if (0 == lRetVal) {
            continue;
        } else if (-1 == lRetVal) {
            break;
        } else if (FD_ISSET(sd, &fdset) != 0) {
            char RecvDataBuff[MAX_DATA_BUF_LEN]; // 接收数据buffer  
            unsigned long ulLen = recvfrom(sd, RecvDataBuff, MAX_DATA_BUF_LEN, 0, NULL, NULL);  
            //... // 处理接收到的数据  
            // 画点

            printf("recvfrom len %ld\n", ulLen);

            // from客户端
            vector<KeyPoint> kp;
            Mat desc;

            // 服务端
            akaze_res = akaze_tracker.process(stats, kp, desc);

            // 展示信息
            //drawStatistics(akaze_res, akaze_draw_stats);
            //cv::imshow("server", akaze_res);

            char buf[128] = "processed";
            socketSend(buf, sizeof(buf));
            printf("send %s\n", buf);
        }

        if(waitKey(1)==27) break; //quit on ESC button
    }

    close(sd);
    printStatistics("AKAZE", akaze_stats);
    return 0;
}
